﻿using System;
using System.Collections.Generic;
using MathNet.Numerics.LinearAlgebra;
using BarStructures.Deformed;
using MathNet.Numerics;
using BarStructures.Utils;

namespace BarStructures.Analysers
{
    /// <summary>
    /// Блочная динамическая конденсация для дискретной матрицы масс и обращением
    /// матрицы жёсткости на первом шаге.
    /// </summary>
    public class BlockDynamicCondensationKinv_XY_DM : Analyser
    {
        private Matrix eigenVectors = null;

        private Vector eigenValues = null;

        /// <summary> Размер блока из группы второстепенных степеней свободы. </summary>
        private int groupSize = 4;

        /// <summary> Рабочие матрицы жесткости и масс. </summary>
        Matrix Kmrg, Mmrg;

        /// <summary> Массив основных индексов. </summary>
        int[] primaryDegrees;

        /// <summary> Массив второстепенных индексов. </summary>
        int[] secondaryDegrees;

        /// <summary> Массив смешаных индексов. </summary>
        int[] mergedDegrees;

        Matrix Tr = null;

        public BlockDynamicCondensationKinv_XY_DM(Model model, string name): 
            base(model, name)
        {

        }

        public override void Analyse()
        {
            if (!UseExistingIndexation)
                model.SetAutoSystemIndexes();

            int modelDimension = model.GetNodesCount() * 2;

            //подготовка матриц
            Matrix stiffness = new Matrix(modelDimension, modelDimension);
            Matrix mass = new Matrix(modelDimension, modelDimension);

            #region Построение матриц жесткости и масс
            Matrix elMx, save;
            int barsCount, ind1, ind2;
            barsCount = model.GetBarsCount();
            for (int i = 0; i < barsCount; i++)
            {
                Bar bar = model.GetBar(i);
                ind1 = bar.NodeOne.SystemIndex * 2;
                ind2 = bar.NodeTwo.SystemIndex * 2;

                elMx = ElementHelper.GetGlobalStiffness6DMatrix(bar);
                save = stiffness.GetMatrix(ind1, ind1 + 1, ind1, ind1 + 1);
                save.Add(elMx.GetMatrix(0, 1, 0, 1));
                stiffness.SetMatrix(ind1, ind1 + 1, ind1, ind1 + 1, save);

                save = stiffness.GetMatrix(ind1, ind1 + 1, ind2, ind2 + 1);
                save.Add(elMx.GetMatrix(0, 1, 3, 4));
                stiffness.SetMatrix(ind1, ind1 + 1, ind2, ind2 + 1, save);

                save = stiffness.GetMatrix(ind2, ind2 + 1, ind1, ind1 + 1);
                save.Add(elMx.GetMatrix(3, 4, 0, 1));
                stiffness.SetMatrix(ind2, ind2 + 1, ind1, ind1 + 1, save);

                save = stiffness.GetMatrix(ind2, ind2 + 1, ind2, ind2 + 1);
                save.Add(elMx.GetMatrix(3, 4, 3, 4));
                stiffness.SetMatrix(ind2, ind2 + 1, ind2, ind2 + 1, save);


                elMx = ElementHelper.GetGlobalDescreteMass6DMatrix(bar);
                save = mass.GetMatrix(ind1, ind1 + 1, ind1, ind1 + 1);
                save.Add(elMx.GetMatrix(0, 1, 0, 1));
                mass.SetMatrix(ind1, ind1 + 1, ind1, ind1 + 1, save);

                save = mass.GetMatrix(ind1, ind1 + 1, ind2, ind2 + 1);
                save.Add(elMx.GetMatrix(0, 1, 3, 4));
                mass.SetMatrix(ind1, ind1 + 1, ind2, ind2 + 1, save);

                save = mass.GetMatrix(ind2, ind2 + 1, ind1, ind1 + 1);
                save.Add(elMx.GetMatrix(3, 4, 0, 1));
                mass.SetMatrix(ind2, ind2 + 1, ind1, ind1 + 1, save);

                save = mass.GetMatrix(ind2, ind2 + 1, ind2, ind2 + 1);
                save.Add(elMx.GetMatrix(3, 4, 3, 4));
                mass.SetMatrix(ind2, ind2 + 1, ind2, ind2 + 1, save);
            }

            //DbgPrint.Matrix(mass, "mass_0.txt");
            //DbgPrint.Matrix(stiffness, "stiffness_0.txt"); 
            #endregion

            #region Индексы основных и второстепенных степеней свободы и смешанные индексы

            List<int> primaryDegreesList = new List<int>();
            List<int> secondaryDegreesList = new List<int>();
            for (int i = 0; i < model.GetNodesCount(); i++)
            {
                Node node = model.GetNodeBySystemIndex(i);
                if (node.DegreeSelection.X)
                    primaryDegreesList.Add(i * 2);
                else
                    if (!node.Support.Sx)
                        secondaryDegreesList.Add(i * 2);
                if (node.DegreeSelection.Y)
                    primaryDegreesList.Add(i * 2 + 1);
                else
                    if (!node.Support.Sy)
                        secondaryDegreesList.Add(i * 2 + 1);
            }
            primaryDegrees = primaryDegreesList.ToArray();
            secondaryDegrees = secondaryDegreesList.ToArray();
            mergedDegrees = new int[primaryDegrees.Length + secondaryDegrees.Length];
            Array.Copy(primaryDegrees, mergedDegrees, primaryDegrees.Length);
            Array.Copy(secondaryDegrees, 0, mergedDegrees, primaryDegrees.Length, secondaryDegrees.Length);

            int primaryLength = primaryDegrees.Length;
            int secondaryLength = secondaryDegrees.Length;
            int mergedLength = mergedDegrees.Length;

            #endregion

            //DbgPrint.Matrix(mass.GetMatrix(mergedDegrees, mergedDegrees), "mass_0.txt");
            //DbgPrint.Matrix(stiffness.GetMatrix(mergedDegrees, mergedDegrees), "stiffness_0.txt"); 

            //Рабочие матрицы, без связанных степеней свободы
            Kmrg = stiffness.GetMatrix(mergedDegrees, mergedDegrees).Inverse();
            Mmrg = mass.GetMatrix(mergedDegrees, mergedDegrees);

            //подматрицы от основных степеней свободы
            Matrix stKrr = Kmrg.GetMatrix(0, primaryLength - 1, 0, primaryLength - 1);
            Matrix stMrr = Mmrg.GetMatrix(0, primaryLength - 1, 0, primaryLength - 1);
            Matrix stKrrInv = stKrr.Inverse();

            //объявление переменных подматриц для расчётов
            Matrix stKrs, stKsr, stKss, stMss, stIs;
            Matrix stK, stM;
            Matrix D;
            //матрица добавок масс
            Matrix mAppendant = stMrr.Clone();

            //порядковые индексы начала и конца блока в группе второстепенных степеней свободы
            int blockStart, blockEnd;
            blockStart = primaryLength;
            blockEnd = blockStart + groupSize - 1;
            if (blockEnd >= mergedLength)
                blockEnd = mergedLength - 1;
            //блочная конденсация
            while (blockStart < mergedLength)
            {
                stKrs = Kmrg.GetMatrix(0, primaryLength - 1, blockStart, blockEnd);
                stKsr = Kmrg.GetMatrix(blockStart, blockEnd, 0, primaryLength - 1);
                stKss = Kmrg.GetMatrix(blockStart, blockEnd, blockStart, blockEnd);
                stMss = Mmrg.GetMatrix(blockStart, blockEnd, blockStart, blockEnd);

                stK = new Matrix(primaryLength + blockEnd - blockStart + 1, primaryLength + blockEnd - blockStart + 1);
                stM = new Matrix(primaryLength + blockEnd - blockStart + 1, primaryLength + blockEnd - blockStart + 1);

                stK.SetMatrix(0, primaryLength - 1, 0, primaryLength - 1, stKrr);
                stK.SetMatrix(0, primaryLength - 1, primaryLength, stK.ColumnCount - 1, stKrs);
                stK.SetMatrix(primaryLength, stK.RowCount - 1, 0, primaryLength - 1, stKsr);
                stK.SetMatrix(primaryLength, stK.RowCount - 1, primaryLength, stK.ColumnCount - 1, stKss);
                stM.SetMatrix(0, primaryLength - 1, 0, primaryLength - 1, stMrr);
                stM.SetMatrix(primaryLength, stM.RowCount - 1, primaryLength, stM.ColumnCount - 1, stMss);
               
                //максимальное собственное значение подсистемы
                save = stK.Multiply(stM);
                EigenvalueDecomposition _evd = new EigenvalueDecomposition(save);
                Vector _eigenValues = _evd.RealEigenvalues;
                double maxValue = _eigenValues[0];
                for (int i = 0; i < _eigenValues.Length; i++)
                    if (maxValue < _eigenValues[i])
                        maxValue = _eigenValues[i];

                stIs = new Matrix(blockEnd - blockStart + 1, -maxValue);

                //Расчёт D
                Matrix save1, save2, save3;
                save1 = stKrs.Multiply(stMss);
                save2 = stKss.Multiply(stMss);
                save2.Add(stIs);
                save2 = save2.Inverse();
                //save2 = MxOp.Inverse(MxOp.Add(MxOp.Product(stKss, stMss), stIs));
                save3 = stKsr.Multiply(stMrr);
                D = save1.Multiply(save2).Multiply(save3);
                D.Multiply(-1D);
                //D = MxOp.Product(MxOp.Product(MxOp.Product(save1, save2), save3), -1);

                //вычисление довеска
                save = stKrrInv.Multiply(D);
                mAppendant.Add(save);

                blockStart += groupSize;
                blockEnd += groupSize;
                if (blockEnd >= mergedLength)
                    blockEnd = mergedLength - 1;
            }
            Matrix mx = stKrr.Multiply(mAppendant);
            EigenvalueDecomposition evd = new EigenvalueDecomposition(mx);
            eigenValues = evd.RealEigenvalues;
            eigenVectors = evd.EigenVectors;
        }

        public override DeformedState GetDeformedStateForEigenValue(double eigenValue)
        {
            List<double> eigenVector = GetFullEigenVecsListForEigenValue(eigenValue);

            if (eigenVector.Count != 0)
            {
                DeformedState deformedState = new FrameDeformedState();
                deformedState.AssignToModel(model);
                int counter = 0;
                double dx, dy;
                for (int j = 0; j < model.GetNodesCount(); j++)
                {
                    Node node = model.GetNodeBySystemIndex(j);
                    dx = eigenVector[counter++];
                    dy = eigenVector[counter++];

                    NodeDisplacement nodeDisplacement = new NodeDisplacement(node, dx, dy, 0);
                    deformedState.Add(j, nodeDisplacement);
                }

                return deformedState;
            }
            else
                return null;
        }

        public override List<double> GetEigenValuesList()
        {
            if (eigenValues == null)
                Analyse();
            List<double> result = new List<double>();
            for (int i = 0; i < eigenValues.Length; i++)
                result.Add(1 / eigenValues[i]);
            return result;
        }

        public override List<double> GetEigenVecsListForEigenValue(double eigenValue)
        {
            if (eigenValues == null)
                Analyse();
            eigenValue = 1 / eigenValue;
            int i = 0;
            while (i < eigenValues.Length && !Number.AlmostEqual(eigenValue, eigenValues[i]))
                i++;
            List<double> result = new List<double>();
            if (i < eigenValues.Length)
            {
                for (int j = 0; j < eigenVectors.RowCount; j++)
                    result.Add(eigenVectors[j, i]);
            }
            return result;
        }

        public override List<double> GetFullEigenVecsListForEigenValue(double eigenValue)
        {
            if (eigenValues == null)
                Analyse();
            eigenValue = 1 / eigenValue;
            int i = 0;
            while (i < eigenValues.Length && !Number.AlmostEqual(eigenValue, eigenValues[i]))
                i++;

            if (i < eigenValues.Length)
            {
                //конденсированный вектор
                Vector cndVec = eigenVectors.GetColumnVector(i);

                //получение недостающих компонентов полного вектора
                if (Tr == null)
                {
                    Tr = new Matrix(mergedDegrees.Length, primaryDegrees.Length);
                    //единичная подматрица
                    for (int j = 0; j < primaryDegrees.Length; j++)
                        Tr[j, j] = 1;
                    //рассчитанное "центральное" собственное значение
                    double pivotEigenValue = eigenValues[0];
                    for (int j = 1; j < eigenValues.Length; j++)
                        if (eigenValues[j] < pivotEigenValue)
                            pivotEigenValue = eigenValues[j];

                    //первый множитель
                    Matrix Kss = Kmrg.GetMatrix(primaryDegrees.Length, mergedDegrees.Length - 1, primaryDegrees.Length, mergedDegrees.Length - 1);
                    Matrix Mss = Mmrg.GetMatrix(primaryDegrees.Length, mergedDegrees.Length - 1, primaryDegrees.Length, mergedDegrees.Length - 1);
                    Matrix save = Kss.Multiply(Mss);
                    save.Add(new Matrix(secondaryDegrees.Length, -pivotEigenValue));
                    save = save.Inverse();
                    //второй множитель
                    Matrix Ksr = Kmrg.GetMatrix(primaryDegrees.Length, mergedDegrees.Length - 1, 0, primaryDegrees.Length - 1);
                    Matrix Mrr = Mmrg.GetMatrix(0, primaryDegrees.Length - 1, 0, primaryDegrees.Length - 1);
                    Matrix save2 = Ksr.Multiply(Mrr);

                    save = save.Multiply(save2);
                    save.Multiply(-1);

                    Tr.SetMatrix(primaryDegrees.Length, mergedDegrees.Length - 1, 0, primaryDegrees.Length - 1, save);
                }

                Vector reBuiltVector = Tr.Multiply(cndVec.ToColumnMatrix()).GetColumnVector(0);

                Vector eigenVector = new Vector(model.GetNodesCount() * 2);
                for (int j = 0; j < reBuiltVector.Length; j++)
                    eigenVector[mergedDegrees[j]] = reBuiltVector[j];

                return new List<double>(eigenVector.CopyToArray());

            }
            else
                return null;
        }
    }
}
